
#include "SceneDrawer.h"
#include <stdio.h>
#include <conio.h>
#include "Kinect.h"
#include <iostream>
#include <windows.h>
#include <winsock2.h>
using namespace std;
static const int BUFSIZE = 5120;

DWORD WINAPI display(void *pArg) {//display thread
	char **argv = (char **) pArg;
	SceneDrawer::glMain(1,argv);
	return DWORD(0);
}


int main(int argc, char **argv) {
	DWORD tid;
	HANDLE _handle = CreateThread(0,
		0,
		display,
		(void*)argv,
		CREATE_SUSPENDED,
		&tid);
	struct hostent *hp;
	struct sockaddr_in saddr, caddr;
	int sock, fromlen, len;
	int port = 21652;


	WSADATA WsaDat;
	if (WSAStartup(MAKEWORD(2,2), &WsaDat) != 0) {
		cerr<<"Couldn't Init"<<endl;
		getch();
		exit(-1);
	}

	if((sock = socket(AF_INET, SOCK_DGRAM,0)) <0) {
		cerr<<"sock create failed"<<endl;
		getch();
		exit(-1);
	}

	memset(&caddr, 0, sizeof(caddr));
	memset(&saddr, 0, sizeof(saddr));
	saddr.sin_family = AF_INET;
	saddr.sin_addr.S_un.S_addr = htonl(INADDR_ANY);
	saddr.sin_port = htons(port);

	if(bind(sock,(LPSOCKADDR)&saddr, sizeof(saddr)) == SOCKET_ERROR) {
		cerr<<"sock bind failed "<<endl;
		perror("Error: ");
		getch();
		closesocket(sock);
		exit(-1);
	}
	int conn = 0;

	fromlen = sizeof(caddr);
	static const int MSGSIZE = sizeof(KinectMsg);
	KinectMsg *msg;
	char *recvBuf = new char[MSGSIZE];
	bool bFirst = true;
	char *tmp = new char[BUFSIZE+2];

	while(1) {
		long totalRead = 0;
		bool bPackageFull = false;
		int i;
		memset(tmp,0,sizeof(tmp));
		for(i = 0; i<(MSGSIZE/BUFSIZE)+1; i++) {
			len = recvfrom(sock, tmp,  BUFSIZE+2, 0,NULL,NULL) -2 ;
			int pack = *(reinterpret_cast<unsigned short*>(tmp));
			//printf("len-%d, pack %d \n",len,pack);
			//len = recv(conn,recvBuf+i*BUFSIZE, BUFSIZE, 0);
			if(len <= 0) {
				cout<<"package "<<i<<" read failed! "<<len<<endl;
				bPackageFull = false;
				break;
			}
			memcpy(recvBuf + pack*BUFSIZE, tmp+2, pack==MSGSIZE/BUFSIZE?sizeof(BodyInfo):len);
			if(len < BUFSIZE && i != MSGSIZE/BUFSIZE) {
				cout<<"package "<<i<<" only read "<<len<<endl;
			}
			totalRead += len;
			//cout<<"Total Read: "<<totalRead<<endl;
			if(totalRead > MSGSIZE) {
				cout<<"totalRead too bid "<<totalRead<<" i = "<<i<<endl;
				break;
			}
		}

		if(totalRead == MSGSIZE) {
			bPackageFull = true;
			//cout<<"Good!"<<endl;
		}
		else {
			cout<<"Package failed"<<endl;
		}

		if(bPackageFull) {	//read all data
			//cout<<"Total Read "<<totalRead<<endl;
			//msg = reinterpret_cast<KinectMsg *>(recvBuf);
			SceneDrawer::msg = *(reinterpret_cast<KinectMsg *>(recvBuf));
			//SceneDrawer::bNewMsg = true;
			if(bFirst) {
				ResumeThread(_handle);
				bFirst = false;
			}
		}
	}
	return 0;
}